#include "fxp.h"
#include "defines.h"
#include "lar_logic.h"

//Quantization of Log-Area Ratios
const int16_t table_A[]={20480,20480,20480,20480,13964,15360,8534,9036};
const int16_t table_B[]={0,0,2048,-2560,94,-1792,-341,-1144};
const int16_t table_MIC[]={-32,-32,-16,-16,-8,-8,-4,-4};
const int16_t table_MAC[]={31,31,15,15,7,7,3,3};

const int16_t table_INVA[]={13107,13107,13107,13107,19223,17476,31454,29708};

const int16_t table_DLB[] = {
6554,
16384,
26214,
32767
};

const int16_t table_QLB[] = {
3277,
11469,
21299,
32767
};

const int16_t table_H[] = {
 -134 ,
 -374 ,
 0,
 2054,
 5741,
 8192,
 5741,
 2054,
 0,
 -374 ,
 -134
};

// TODO check table types

const int16_t table_FAC[]={
 18431, 20479, 22527, 24575, 26623, 28671, 30719, 32767
};

const int16_t table_NRFAC[]={
 29128, 26215, 23832, 21846, 20165, 18725, 17476, 16384
};

// TODO fix includes in all files
// Decoding of the coded Log-Area Ratios(5.2.8)
void larc_to_larpp(int16_t* LARc,int16_t* LARpp) {
	int i;
	int16_t temp1,temp2;
	for(i=1;i<9;i++) {
		// Used to restore the sign of LARc[i]
		temp1=fxp_add(LARc[i],table_MIC[i])<<10;
		temp2=table_B[i]<<i;
		temp1=fxp_sub(temp1,temp2);
		temp1=fxp_mult_r(table_INVA[i],temp1);
		LARpp[i]=fxp_add(temp1,temp1);
	}
}

// Computation of the quantized reflection coefficients(5.2.9)
/// Interpolation of the LARpp to get the LARpi(5.2.9.1)
void larpp_to_larp(int16_t *LARpp,int16_t *LARp,int k,int16_t* prev_LARpp) {
//	static int16_t prev_LARpp={0,0,0,0,0,0,0,0};
	int i;
	if(k==0)
		for(i=1;i<9;i++) {
			LARp[i]=fxp_add(prev_LARpp[i]>>2,LARpp[i]>>2);
			LARp[i]=fxp_add(LARp[i],(prev_LARpp[i]>>1));
		}
	else if(k==1)
		for(i=1;i<9;i++) {
			LARp[i]=fxp_add(prev_LARpp[i]>>1,LARpp[i]>>1);
		}

	else if(k==2)
		for(i=1;i<9;i++) {
			LARp[i]=fxp_add(prev_LARpp[i]>>2,LARpp[i]>>2);
			LARp[i]=fxp_add(LARp[i],(prev_LARpp[i]>>1));
		}

	else if(k==3) {
		for(i=1;i<9;i++)
			LARp[i]=LARpp[i];
		// Retain LARpp for next iteration
		for(i=0;i<8;i++)
			prev_LARpp[i]=LARpp[i];
	}
}

// Computation of the rp[1..8] from the interpolated LARp[1..8] (5.2.9.2)
void larp_to_rp(int16_t* LARp,int16_t* rp) {
	int16_t temp;
	int i;
	for(i=1;i<9;i++) {
		temp=fxp_abs(LARp[i]);
		if(temp<11059) temp<<=1;
		else if(temp<20070) temp=fxp_add(temp,11059);
		else temp=fxp_add((temp>>2),26112);
		rp[i]=temp;
		if(LARp[i]<0)
			rp[i]=fxp_sub(0,rp[i]);
	}
}

// Short term analysis filtering(5.2.10)
// TODO : reimplement this
void st_filter_partial(int16_t* s,int16_t* d,int16_t* rp,int16_t* u,int k) {
	int16_t di,sav,temp;
	int k_start,k_end;
	int i,j;

	switch(k) {
		case 0:k_start=0;k_end=12;break;
		case 1:k_start=13;k_end=26;break;
		case 2:k_start=27;k_end=39;break;
		case 3:k_start=40;k_end=159;break;
		default : return;
	}


	for(i=k_start;i<=k_end;i++) {
		di=s[i];
		sav=di;
		for(j=1;j<=8;j++) {
			temp=fxp_add(u[j-1],fxp_mult_r(rp[j],di));
			di=fxp_add(di,fxp_mult_r(rp[j],u[j-1]));
			u[j-1]=sav;
			sav=temp;
		}
		d[i]=di;
	}	
}

void st_filter(int16_t* s,int16_t* d,int16_t *LARpp,int16_t* prev_LARpp,int16_t* u,int16_t* rp,int16_t* LARp) {
	// static int16_t u[8]={0,0,0,0,0,0,0,0};
	// static int16_t rp[8]={0,0,0,0,0,0,0,0};
	// static int16_t LARp[8]={0,0,0,0,0,0,0,0};
	// For k_start = 0 to k_end = 12
	int k;

	for (k = 0; k < 4; k++) {
		larpp_to_larp(LARpp, LARp, k, prev_LARpp);
		larp_to_rp(LARp, rp);
		st_filter_partial(s, d, rp, u, k);
	}
	// For k_start = 13 to k_end = 26
//	larpp_to_larp(LARpp,LARp,12,prev_LARpp);
//	larp_to_rp(LARp,rp);
//	st_filter_partial(s,d,rp,u,13);
//	// For k_start = 27 to k_end = 39
//	larpp_to_larp(LARpp,LARp,27,prev_LARpp);
//	larp_to_rp(LARp,rp);
//	st_filter_partial(s,d,rp,u,27);
//	// For k_start = 40 to k_end = 159
//	larpp_to_larp(LARpp,LARp,40,prev_LARpp);
//	larp_to_rp(LARp,rp);
//	st_filter_partial(s,d,rp,u,40);
}

